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ABSTRACT 


The use of Unmanned Aerial Vehicles (UAVs) in modern military operations for 
reconnaissance and other missions | continues to grow. UAV systems using remote control 
guidance are limited in range and subject to Electronic Warfare concerns. Guidance systems 
using only Global Positioning Service (GPS) or an Inertial Navigation System (INS) are limited 
to a pre-programmed route of flight. A vision. guidance system that can control the UAV over an 
arbitrary course is not subject to these limitations. This thesis uses classical control methods to 
develop and test an autonomous vision controller for the FOG-R UAV (FROG). First, a 
_ computer model of the camera output for a flight that tracks a river is made to develop the 
controller and to test it in nonlinear simulation. Finally, the complete system is flight tested on 
the FROG UAV. 

The design and test equipment include a highly modified FOG-R UAV from the U.S. 
ines: the MATRIXx Product Family of software tools developed by Integrated Systems, Inc., 
and a Ground Station built at NPS from commercially available computer and communication 


equipment. 
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I. INTRODUCTION 


A. PURPOSE 


The primary er of this thesis was to design, simulate on the ground and flight 
test an autonomous vision guidance system for an Unmanned Aerial Vehicle (UAV). To 
achieve this, a model of the vision sensor was also developed. The vision guidance system 
controls the UAV in the horizontal plane to follow an arbitrary curve on the ground. The 
guidance control apie was designed to take the output from a digital image processor to 
track the arbitrary curve. This processor uses the image obtained by an onboard video 
camera in the nose of the aircraft. The image processor uses color to distinguish a 
contiguous curve, such as a river or road, in sal picture frame sent to it from a digital 
frame grabber. The processor then takes the pixel coordinates of this curve in the camera 
image plane that corresponds to a pre-determined distance ahead of the UAV and sends it to 
the controller. A computer model for the camera and processor sensor unit was needed to 
develop the controller and to test it during ground simulations. Additionally, the digital 
image processor programming and hardware interfaces are still under development 
necessitating the use of the camera model during the actual flight testing of the controller. 

The secondary purpose of this thesis was to implement and flight test improvements 
to the existing Flight Management System (FMS) for the Naval Postgraduate School’s 
UAV... The FMS has been evolving as previous thesis students have developed individual 
controllers for altitude, speed, and heading. Integrating these different controllers for 


combined operation during flight testing was needed to improve safety of the test vehicle 





and to simplify use for the operator. Changes to the FMS include coding changes to the 
user defined blocks of the existing FMS software, changes to the wind down filters, and the 


addition of switches to choose between absolute or delta commands. 


B. HARDWARE 


Only a brief description of the hardware involved is required for the presentation of 


this project; however, a comprehensive description can be found in Froncillo, Komlosy, and 


Rivers [Refs. 1-3]. 





Figure1 FROG UAV | 
1. The Airborne Components 


The flight vehicle used in the design and testing of this controller was the U.S. 


Army’s FOG-R UAV. Nicknamed the FROG (Figure 1), it is a high wing monoplane with 
the engine mounted on a pylon above the twelve foot wingspan. Takeoff weight is typically 


90 lbs., including 20 ibs. of payload used for the sensor package. The FROG uses 


conventional controls of ailerons, elevator, and rudder. The aircraft is Radio Controlled 








(RC) via Futaba Pulse Width Modulation (PWM) transmitters of the type used by sport RC 
modelers. The FROG also has an onboard autopilot with its own yaw and climb rate 
sensors. This allows the pilot to fly the aircraft via direct control surface movement or by 
commanding turn and climb rates through the autopilot. The same control options apply to 
the FMS. 

The sensor package consists of a pitot-static system for airspeed and altitude, five 
single turn potentiometers for the positions of control surfaces and relative wind angles, a 
differential Global Positioning Service (GPS) unit, and an Inertial Measurement Unit 
(IMU). The IMU measures body angles, angle rates, accelerations, and the earth’s magnetic 
field for all three axes of the aircraft. The IMU also has five Analog to Digital (A/D) 
converters that are used to send velocity and four other parameters to the ground station. 
The parameters from the potentiometers and pitot-static system that are available for 
transmission are angle ig attack, side slip, the three control surfaces position, dynamic 
pressure, and static pressure. For this project only dynamic pressure, static pressure, and 
side slip sensors were connected to the IMU. All data from the IMU and the differential 
GPS is transmitted to the ground station by two spread spectrum radio frequency (RF) 
modems at a 9600 Baud data rate. The GPS modem operates in the full duplex mode 
receiving differential corrections from the ground station’s GPS receiver. Figure 2 shows 


the hardware layout and interfaces. 
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| Figure 2 Hardware Interfaces 
Zz The Ground Components 


The ground station is responsible for flight control of the UAV and data collection. 
As seen in Figure 2, the ground station consists of three major components. The SPARC 2 
workstation executes all the software used for initial development, ground simulation, and 
control of the aircraft during flight test. All the data storage and analysis is also performed | 
on the SPARC 2. The luggable computer contains the host processor and the AC-100 
Model C30 real-time hardware controller. The host computer handles File Transfer 
Protocol (FTP), compile, link, and download functions of the system. The luggable also has 
four IP’ modules that provide the ‘sadn with the communication box as shown in Figure 


2. The communications box with the associated antennas has all the equipment to transmit 


and receive data and control commands between the ground station and the UAV. This 











includes two RF modems, a GPS receiver, and a Futaba PWM receiver identical to the ones 
in the aircraft. The FROG is controlled using two Futaba RC controllers. One controller, 
referred to as the “slave”, has been modified to accept inputs from the computer via the 
IP_DAC module. The pilot uses a standard Futaba controller as the “master” controller. 
When the pilot holds down the trainer switch on the “master”, control is passed to the 


“slave” and, hence, the computer. 


C. RAPID PROTOTYPING | 


As with the description of the hardware components, the use of RealSim for the 
design of control systems was explored in earlier thesis projects; consequently, only a brief 
summary of the rapid prototyping system is provided below. A more complete description 
can be found in Froncillo, Komlosy, and Rivers [Refs. 1-3]. 

A rapid prototyping system allows the engineer to quickly design, test, and 
implement a control system. The MATRIX, Product Family of software tools is a 
commercial system that provides a a of integrated tools to accomplish this task. The 
functionality of each MATRIX, tool is shown in Figure 3. Xmath/SystemBuild is an 
interrelated software pacKage similar to MATLAB/Simulink. The RealSim Graphical User 
Interface (GUI), shown in Figure 4, provides overall control by stepping the user through 
the design process from initial formulation to the actual real-time implementation of the 


control system. 


Analysis/Design 


Generation 


. 


RealSim | Hardware-in-the- 
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Flight Testing 


Figure 3 RealSim Functions 





Xmath is the computational ciement that provides analysis and control simulation 
functions. SystemBuild is a graphical, interactive program that uses both pre-defined and 
user defined blocks to model system elements. The autocode feature is a powerful time 
saving capability that generates high level C code based on the system built in the graphical 
System/Build environment. Once the code is generated it is sent to the host computer via 
FTP. The host computer compiler seneiaies the object code and the link produces the 
executable code for the target processor. The animation builder enables the user to build a 


graphical interface with the control system that allows real-time inputs as well as display of 




















system outputs during both ground and flight testing. The hardware connection editor iS 


used to associate system inputs and outputs with external hardware. The final feature of the 


RealSim GUI is the download and run feature, which loads the executable code into the 


target processor and initiates the real-time operation. 
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Figure 4 RealSim GUI 











Il. FLIGHT MANAGEMENT SYSTEM 


The FMS is used to sinttel the FROG during flight tests. The animation page, 
shown in Figure 5, allows the user to monitor all the critical flight parameters and to 
command inputs via the different controllers. The FMS has been modified to incorporate 
an Absolute or a Delta mode of operation for both the altitude and the heading controllers. 
All commands entered from the FMS animation page are converted to analog voltages that 
are sent to the Futaba controller. They are then converted to PWM signals and transmitted 
to the FROG. The interfaces are described in the hardware section of Chapter I. It should 
be noted that none of the calleeniads characteristics of the different controllers has been 
altered, and therefore no scniysis of | the performance is done. The changes only affect the 
implementation of the controllers and were ite to enhance safety of the vehicle and to 
ease operation by the user. Flight test and ground simulations were made only to test proper 


implementation. 


A. ALTITUDE CONTROLLER 


The altitude controller for the FROG was developed to send climb rate commands 
through the onboard autopilot [Ref. 1]. The original controller had two modes of operation: 
Open Loop (OL) or Closed Loop (CL). For the OL mode the operator enters the desired 
a rate in feet per minute on the FMS page. No altitude or climb rate is referenced or 
fed back to the controller. The OL mode has not been changed and is still available to the 


operator. 
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Figure5 FMS Page 


The original CL mode consisted only of a Delta altitude mode. The desired altitude 
change in feet was entered on the FMS page. The reference pressure sidiesias at the time the 
altitude CL wail was saned ‘on was frozen and added to the altitude change commanded. 
This altitude sum was compared to the actual pressure altitude and fed back through a 
Proportional-plus-Integral-plus-Derivative (PID) controller that commanded the required 
climb rate to achieve the desired change in altitude. The addition of an absolute altitude 
mode and the Master switch requires a change to this logic. Now the reference altitude is 


not frozen until the Master switch is on, the altitude CL switch is on, and the altitude 
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Absolute/Delta switch is in the Delta position. This prevents the reference altitude from 
being set when operating in the absolute mode, and it allows the subordinate altitude CL 
switch to be set prior to the test run and activated via the Master switch. The reference 
altitude can be cleared and reset by cycling any of these three switches. 

An absolute altitude mode was added that allows the operator to command an 
altitude based only on the pressure altimeter. The resulting ithe command is either an 
above eround level (agl) altitude if the altimeter is zeroed prior to flight, or a mean sea level 
(msl) altitude if the field elevation is set into the altimeter prior to flight. An altitude of 250 
feet is set as the minimum possible commanded altitude as a safety feature. With a test . 
field elevation of 80 feet, this provides adequate ground clearance for both agl and msl 


altimeter settings. 


Absolute Altitude Controller 





900 : 

: | : Press Alt 
> si 3 | ——— Abs Cmd 
@ : . :. : 
= 700 
= 
z 600 
<f 

oU0 
ano : 
10 20 30 40 a0 
- timefseconds) 


Figure 6 Absolute Altitude Test 
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Figure 6 shows the response during a flight test run of the eects altitude mode. 
At the start of the run the FROG was at 480 feet agl and was immediately commanded to . 
700 feet agl. The data shows the FROG climbing and maintaining the desired altitude. The 
default mode at system initiation is the Delta mode with a change of zero feet commanded. 
This is a safety feature that minimizes the possibility of an abrupt altitude change when the 
computer is given control of the aircraft. Figure 7 shows the new altitude controller with 


the logic blocks added to implement these features. 
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Figure 7 Altitude Controller 


B. HEADING CONTROLLER 


The heading controller for the FROG has an OL mode of operation similar to that 
described under the altitude controller section. It commands a yaw rate to the FROG 
autopilot directly from the value entered on the FMS page. No changes were made to the 


OL mode. 


LZ 








The original CL heading mode consisted only of an Absolute mode. The desired 
heading in degrees Boni 0 ‘ 359 is entered on the FMS page and compared to the actual 
heading from the IMU. This is sent through a PID controller that calculates the commanded 
yaw rate required to achieve the desired heading [Ref. 3]. This mode is still available when 
the heading Absolute/Delta switch : in the Absolute position. A new Delta mode was 
added with the same logic as in the altitude controller. A reference heading is frozen when 
the Master switch is on, the heading CL switch is on, and the Absolute/Delta switch is in 
the Delta position. The reference heading can be cleared and reset by cycling any of these 


three switches. The default mode at system initiation is the Absolute mode with 360° 
commanded. With 360° entered the yaw rate commanded is zero. This is a safety feature 


that prevents abrupt commands when the controller is engaged and also allows a zero yaw 
rate command, or “steady up” signal, to be sent during CL operation. 


Figure 8 shows the results of a test run for a Delta command of right 45°. When the 
CL switch was thrown at six seconds, the reference heading of 225° was frozen. The 
controller then turned the aircraft to 270° and maintained that heading. Figure 9 shows the 
— test to the left. At two seconds the reference heading of 240° was frozen: The 
controller turned the aircraft to’ 195° and maintained that heading. Figure 10 shows the new 


heading controller with the logic blocks added to implement these features. 
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Figure9 Left 45 Degrees 
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Figure 10 Heading Controller 


C. WIND DOWN FILTERS 


The Master switch has been added to each wind down filter to ensure proper 
operation. All cL control switches are now subordinate to the Master switch. This allows 
the individual controllers that we going to be tested during a run to be selected ahead of 
time. They are then turned on and off simultaneously via the Master switch. Figure 11 
shows the wind down filter used in the heading contwellen This sonneaeaeon is the same 
as the altitude controller. The wind down loop incorporates all three switches connected to 
each controller: the Trainer Sitch the Master switch, and the CL switch for each 


individual controller. The integrator forces the output back to its initial value if any of the 
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three switches are off. This prevents the control command from building up while the 


controller is not selected. 


Master switch 
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Figure 11 Winddown Filter 
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Ill. COORDINATE SYSTEMS 


The development of the camera model and the vision controller requires an 
introduction of several different coordinate systems, including Earth Centered Earth Fixed 
(ECEF), Local Tangent Plane (LTP), Body, Camera, and Image plane systems. This chapter 
will address the relationships and the transformations between the various coordinate 


systems. 


A. ECEF AND LTP {U} 


Consider a Cartesian coordinate system with its origin at the center of the earth, 
oriented such that its z-axis is aligned due north, its x-axis intersects.the prime meridian, 
and its y-axis completes the right hand rule. This coordinate system is called ECEF [Ref. 
4]. LTP coordinate systems are defined by a plane that 1s tangent to a point on the surface 

of the earth. The point of tangency is defined as the origin of the LTP aes system. 
The x-axis is in the plane and points toward true north. The y-axis is in the plane 
perpendicular to the X-axis and points toward east. The z-axis is perpendicular to the plane. 
If the z-axis points away from the center of the earth as shown in Figure 12, it is referred to 
as a North-East-Up (NEU) orientation. If the positive z-axis points towards the center of 
the earth it is referred to as a North-East-Down (NED) orientation. The NED orientation 
will be used since it is a right hand system and easily corresponds with the standard aircraft 


Body system to be defined later. 
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Figure 12 ECEF and LTP 


Since both the aircraft’s position given by the GPS and the location of the LTP 
origin are specified by latitude and longitude, it is necessary to convert these to ECEF 


coordinates. Let [A1, Az, A]' be the position of any arbitrary point, where 
A, = degrees of latitude, 
Az = degrees of longitude, 


and 
h = height in meters. 
The height of the point is measured above the surface of the earth. GPS uses an ellipsoidal: 


model for the earth based on the World Geodetic Survey of 1984 (WGS84) [Ref. 4]. The 
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two parameters that describe an ellipsoid are its eccentricity (€) and the length of its semi- 
major axis (a). For the WGS84 ellipsoid these values are € = 0.00669437999013 and 

a = 6,378,137 meters. With these values the distance from the center of the WGS84 
ellipsoid to a point on its apie where the local latitude is A;, is given by 


ee (1.1) 


1-7 sin?(Z,) 
and the ECEF coordinates of the aint point are 


x =(N +h)cos(A, )cos(A, }. | 
= (N +h)cos(A, )sin(A, ). (11.2) 

va(iiee )+ h)sin(A, }. ; 
| Since all calculations in the sensor model are done in LTP coordinates, the ECEF 
coordinates are transformed to LTP. 
Let 

[Ai1o» Azo, Mo)’ = the surveyed position of the origin of the LTP, 

Pp = the position of the point in ECEF coordinates, 

Po = the position of the LTP origin in ECEF coordinates, 


Pp tp = the position of the point relative to the LTP origin resolved in ECEF, 


LTP PR — the rotation matrix from ECEF to a NED LTP» 


-sin(A,,)oof4,) —sin(,,)sin(4,) cos) 
—sin(Z,) cof ,) 0 | (HL3) 
—cos(4,, )oo,) —cos{4,,)sin(4,) —sin(’,,) 


-IPp, = the position of the point in LTP coordinates. 
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Then, 


Pp trp = Pp- Po, (IT.4) 
and 


LIPPp= ere R Pp ur. (II1.5) 


B. BODY REFERENCE FRAME {B} 


The aircraft Body reference frame is a right hand orthogonal coordinate system with 
the origin at the aircraft’s center of gravity. © The x-axis points forward along the 
longitudinal axis. The y-axis points out toward the right wing, and the z-axis points down 


completing the right hand rule. Figure 13 shows the Body reference frame. 





Figure 13 Body Reference Frame 


Euler angles are used to define the orientation of two coordinate systems with 
respect to each other. These angles define how the Body reference frame fixed to the 


aircraft is oriented with respect to the inertial LTP reference frame. The angles shown in 


Figure 14 are @ for roll, @ for pitch , and y for yaw. 
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Figure 14 Euler Angles 


There are many possible combinations of rotation matrices. The rotation matrix 
used with conventional aircraft to transform from {U} to {B} is the 3-2-1 rotation matrix 


that rotates in yaw first, then pitch, and then roll. 


cad yjoos sin yoo ~sinl9 
AR=| ceo SMS siya) sr sin(Psin(y) +o)  cos(Asin(g) | (IIL.6) 
AYP P+sinYsiP sn(PooPsin(y)—cos(ysin() casos) 


The inverse of this matrix is used to convert from {B} to {U}. 


cooly) —cosPsin(y)+sin( Asin Ooos(y) sin(P)sin(y)+cos(P)sin(Aoos(y) 
CR=| cH Asily) caMomYtsiN Hsin Asin(y) —sin(Pcos(y)+co(Psin(siny)| (IL7) 
—sin(®) sin(p)oos(O) cox Pcas(O) 
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C. CAMERA REFERENCE FRAME {C} 


The Camera reference frame is a Cartesian coordinate system with its origin located 
at the focal point of the camera. The x-axis points forward along the optical axis of the 
camera. The y-axis is positive to the right, and the z-axis is positive down as shown in 
Figure 15. With the camera mounted in the aircraft, the conversion from {B} to {C} would 


use the same 3-2-1 rotation matrix, £R, as Eq. 101.6. If the camera is mounted coincident 


with the aircraft body axes, then all the Euler angles are zero, the rotation matrix is the 
identity matrix, and {C} would equal {B}. For this application the only non-zero Euler 


angle is Oc that accounts for the depression angle of the nose mounted camera. 


| {C} {B} 





Figure 15 Camera Reference Frame 
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D. IMAGE PLANE {IM} 


To model the camera required transforming the three dimensional reference system 
{C} to a two dimensional camera Image plane reference system. A pinhole camera model 
as shown in Figure 16 and discussed in Kaminer [Ref. 4] was used to accomplish this 


mapping of three dimensional coordinates to a two dimensional system. 


anee > “Pec 
[x,y,z]" 





Figure 16 Image Plane Reference Frame 
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Let f be the focal length of the camera, [¥, v]' denote the projection of “Pec onto the Image 


plane, and suppose CP.c = [xy,z]' . Then 


ep me 
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IV. SENSOR MODEL 


The development of the camera model was pivotal to the design of the vision 
controller. Not only was it required for the design phase, it was used in all the simulation 
and flight testing done during this project. The coordinate transformations from Chapter I 
were used extensively. The purpose of the model was to simulate the output from a camera 
‘mounted in the nose of the aircraft while it was tracking a curve on the ground. The 


ultimate goal was to simulate an “S” turn in the river next to the flight test airfield. 


A. TRANSFORMATIONS 


The sensor model transformations take a point along a curve given in LIP 
eee and convert it into the two dimensional output of the camera model. 
Let 
Pz = the position of the origin of {B} seslued in {U} (position of the aircraft), 
Pp = the solide of the point on the curve resolved in {U}, | 
®P. = the position of the camera resolved in {B}, 
Pc = the position of the camera resolved in {U}, 
Ppc = the relative positing of the point with respect to the camera ensives in {U}, 
“Pp = the relative position of the point with respect to the camera solved in {C}, 
R = the various rotation matrices (Eqs. Il.6, f.7), 


TU “Prc) = the projection mapping of Poco resolved in {IM} (Eq. I1.8). 


Z 





Figure 17 shows the relationship of the vectors with respect to the origin of both the 


LTP {U} and the Body reference frames {B}. 





Figure 17 Position Vectors 


From vector algebra it can be seen that 

Po=Ppt+ UR "Pc, (IV.1) 
and 

Ppc = Pp - Pc. | qV.2) 
The coordinates of the point Ppc are then converted from inertial to Camera references 
frames by 

— Poo = ER GR Pre. (IV.3) 

These are the three dimensional eatin of the point as seen by the camera. The final 


transformation takes this result and converts it to the two dimensional Image plane 
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coordinates that are used by the controller. Using the pinhole camera model these 


i = n(Ppc). 
Vv 


Figure 18 shows the SystemBuild block diagram of the implementation of these equations. 


coordinates are given by 


(IV .4) 


The noise source was added for test simulations. 
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Figure 18 Camera Model 


LTP CURVE POINTS 


The process for calculating the LTP coordinates of the point on the curve varied 
depending on the type of curve being simulated. The initial implementation used a straight 
line as the desired ground track. Points along a circle and a sine wave were also calculated 
to simulate a non-linear track. The final simulation used a series of straight lines to 


simulate the course of the river shown in Figure 19. 
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Figure 19 Flight Test Area 
Each case used the aircraft’s LTP position to calculate the LTP coordinates of the 


closest point on the curve. When the closest point is known, a point with a visibility 
distance of 1000 feet along the curve ahead of the aircraft is calculated. This is the LTP 

control point that is transformed into the Image plane and used by the controller. To ensure 
that the control point is in the simulated camera’s field of view, the visibility distance must 
be calculated based on the field of view of the camera, the camera depression angle, and the 
altitude of the aircraft. This distance also increases the controller’s stability, which will be 
discussed in Chapter V. Figure 18 shows the SystemBuild block diagram used when 


simulating a straight line. The user defined block titled “calc pt LTP” performs the control 


28 











point calculations. It takes as inputs the aircraft’s position, the two points that define the 
line, and the direction of flight. The outputs are the LTP coordinates of the control point, 
the range to the line, and the coordinates of the closest point on the line. The last two 
parameters are used only to evaluate performance. 
Let 
(Xp, Yp]’ = the eaepatal plane LTP coordinates of the FROG, 
[X-,¥e]' = the horizontal plane LTP coordinates of the closest point on the line, 
[XeYal’ = the horizontal plane LTP coordinates of the soni point, 
(X1,Y;]' = the horizontal plane LTP coordinates of point one, 
[X2,Y.]' = the horizontal plane LTP coordinates of point two, 
| d = the visibility distance, 
and 
m = the slope of the line. 
The closest point on the line to the FROG is found by the intersection of the line defined by 
the two points and the line perpendicular to it that passes through the FROG’s position. 
Using the point slope equation for a line, the intersection sins are given by 
Yo=(Yr*m/’2 + Xp*m — X2*m + Y2)/(1 + m2), (IV.5) 
and 
Xc=(Yo~Yo)/m + Xo, | (IV.6) 
The control point coordinates are given by 
Xa = Xc + d*cos(tan"(m)), (IV.7) 


and 
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Y, =m*(X,q -Xc) + Ye. (IV.8) 


Similar user defined blocks are used to simulate a circle and a sine wave. The Xmath code 


used for the line, circle, and sine wave calculations are found in the Appendix. 


C. RIVER SIMULATION 


For the case of tracking of a single line, these points were fed directly into the “calc 
pt LTP” block. To simulate the river, however, required additional reference frame 
conversions and switching logic to approximate the desired “S” turn by the airfield. Figure 


20 shows the SystemBuild blocks that perform these functions. 
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Figure 20 Line Points 


The four points that define the three lines shown in Figure 19 were entered by their 
latitude and longitude as well as the position of the origin of the LTP. The points were 


converted to LTP coordinates in the blocks titled “Itp_conv” using Eqs. II.1 through Ii.4. 
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At system initiation points one and two were used to define the line. When the specified 
distance to point two was reached, points two and three were used to define the line. When 
the specified distance to point three was reached, points three and four were used to define 
the line. For ease of calculation, the distance to each of these switching points was done 
using LTP coordinates in the user defined block titled “dist pt”. The logic to switch 
between these points was made through the use of a state machine titled “line switch” and 
two user defined blocks titled “switch Ptl Pt3” and “switch Pt2 Pt4”. When the conditions 
specified in the state machine to switch between points are met, it sends the signal to the 


user defined blocks to switch from point one to point three or from point two to point four. 


D. STATE MACHINE 


A state diagram block performs logical operations on the inputs defined by the user. 
These logical operations determine the transition criteria between the different states of the 
state machine. Transitions between the states ii occur in the direction spectfied by the 
user. The state machine will not een a previous state unless the user specifically sets 
up a transition in that direction with its own transition conditions. In other words once the 
conditions are met to transition from one state to another, the state machine will remain at 
the new state even if the transition condition becomes invalid. In this application this 
feature allowed the use of secresine aircraft range to the switching points as the transition 
criteria between the states. The state machine does not revert to tracking the previous line 
even after the aircraft passes the switching point and the range increases to where it no. 


longer meets the original transition condition. 
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The state diagram block can have any number of outputs. Al! the outputs are 
Boolean in nature with a value of one for True and zero for False. In this application the - 
outputs are used as flags in the user defined Xmath code blocks to switch between the four 
points used to define the three lines. The outputs can be set to True or reset to False any 

number of times. Also, the outputs are of two different types depending on where they are 

set or reset. An output that is changed upon reaching a new state i called a Moore output. 
An output that is changed during the transition between states is called a Mealy output. The 
Mealy output can be used when a state has more than one path ieadine to it, ensuring that 
the same conditions are set by the machine at the arrival at a given state no matter what path 
is followed getting there. 

. The diagram shown in Figure 21 is the state machine used to switch between the 
three lines. At system initiation all outputs are set to False. With both outputs False the 
user defined switching blocks output points one and ees which define line one. When the 
vision controller switch is on and the FROG is less then 700 feet to the first line, the 
machine transitions to the first state labeled “Track line 1”. No outputs are changed at this 
state. When the FROG is less than 600 feet to point two, the sella transitions to state 
two labeled “Track line 2”. At this state a Moore output is used to set output one to True. 
With output one set to True the switching blocks will output points two and three, which 
define line two. When the FROG is less than 600 feet to point three, the machine 
transitions to state three labeled “Track line 3”. At this state both outputs one and two are 
set to True. With both outputs set to True the switching blocks will output points three and 


four, which define line three. 
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Figure 21 State Machine 


If the vision controller switch is turned off at any time, the machine transitions back 
to the initialization or When this happens from state one, no outputs are reset since none 
were Set at state one. When this ects from state two, a Mealy output is used to reset 
output one to False, since this was the only output set upon arrival at state two. When this 
occurs from state three, Mealy outputs are used to reset both outputs one and two to False. 
This demonstrates the use of Mealy versus Moore outputs. The transition back to the 
initialization state means that the points for line one are again used to define the desired 


track. Thus, the resetting of the vision controller is accomplished without having to turn off 


the entire RealSim controller operation, which is an important operational feature. 
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V. VISION CONTROLLER DEVELOPMENT 


A. DESIGN CONCEPT 


Let [u,v]" denote the Image plane coordinates of the control point along the line 
representing the river. Suppose u = 0, then in straight and level flight the <a plane of the 
both the aircraft’s Body and Camera coordinate systems are coincident with the vertical 
plane that contains both the point along the river and the aircraft. As shown in Figure 22, 
this places the control point directly in front of the aircraft. Additionally for the case of a 


straight line, if u = 0 and du/dt = 0, then the aircraft is on the line. 


| u=0 
Cont Point 


{IM} 


Figure 22 Geometry of u - 0 


Therefore, the control strategy was to drive u to zero using yaw rate command as shown in 


Figure 23. 





Definition 





Yaw Rate Command 


Figure 23 Control Strategy 
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With this design concept in mind, the vision controller design requirements are: 


A wWoN 


i 


Seamless Transition — no large fluctuations when engaged. 

Automatic Capture and Track of the desired course. 

Integration into the FMS that allows use of existing controllers. 

Stable Feedback system. 

Transient performance rapid enough to be evaluated in the confines of the flight 
test area. 

Steady State errors small enough to keep the desired track in the field of view of 
the camera. 


6 dB positive Gain and 45° Phase stability margins in the control loop. 


_ iB. DESIGN METHOD 


The control system design approach used was the root-locus method described in 


Ogata [Ref. 5] aided by the computational capability of Xmath. This method uses a 


graphical approach for finding the position of the closed-loop poles based on knowledge of 


the locations of the open-loop poles and zeros. If the desired performance can not be 


achieved by merely adjusting the gain in the feedback loop, adding a compensator in the 


feedback system reshapes the root loci. The compensator adds to the system additional 


poles and/or zeros that influence the shape of the root loci so as to meet the desired 


performance. This is a trial and error approach to system design that is greatly enhanced by 


Xmath, which can quickly recalculate and display the root-locus plot for changes to the 


system. 
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C. CONTROLLER DESIGN 
1. Initial Design 


The model for the FROG/Autopilot plant combination was developed in 
Papageogiou [Ref. 6] and has been used in many prior theses and class projects. The sensor 


model and FROG/Autopilot plant combination is shown in Figure 24. 
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Figure 24. FROG/Autopilot/Sensor Plant 


The sensor model for the initial design used a single straight line for the desired - 
track. Using this model the system was linearized around a trim point with the FROG in 
straight and level flight with a velocity of approximately 50 knots. The open-loop 


eigenvalues are shown in Table 1 and the root-locus plot in Figure 25. The eigenvalues of 
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concern are the complex pair with a damping of 0.41 and frequency of 0.72, which quickly 


migrate to the right half plane causing the system to go unstable. 


Frequency (rad/sec 


Eigenvalue Damping 

0 -1.0 0 

0 -1.0 0 

0 -1.0 0 

0 -1.0 0 
-0.1687 1.0 0.1687 
-0.2972+0.65941 0.4109 0.7232 
-2.9602 1.0 2.9602 
-4.0115 1.0 4.0114 
-1.6310+3.81231 0.3933 4.1466 
-0.8426+4.15931 0.1985 4.2437 


Table 1 OL Plant Eigenvalues 


Imaglnary 





Figure 25 Root-locus OL Plant 


The initial sensor model used the closest point on the curve from the aircraft as the 
control point. The rationale was to try to drive the FROG to the closest point on the line. 
This approach is not desirable for two reasons. The first is that when the FROG is properly 


tracking the line the control point is directly underneath the aircraft. Though this point 
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called the Nadir theoretically exists in {IM}, it would not be in the field of view of the 
camera. The second ee with this approach is that it produces a nonminimum phase 
zero very close to the origin. A zero at this location makes it hard to control the poles of 
concern. The final control point uses a visibility distance of 1000 feet along the curve in 
front of the FROG. This places it the camera field of view and moves the zero to .0656 as 
shown in Figure 26. Though this is still a nonminimum phase system, it allows the poles of 


concern to be controlled as desired. 


Imaginary 
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Figure 26 Root-locus New Sensor 
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With the visibility distance set, a single pole and zero compensator with the transfer 
function of 


(V.1) 


was added to the system. This adds a zero at positive 10 and a pole at —1.0 that.cancels a 
zero from the plant at that location. The effect to the root-locus plot is shown in Figures 27 
and 28. The eigenvalues of concern are drawn down towards the Real axis and remain 
stable for much greater values of the gain. The gain shown in Figure 28 is 75. The closed- 
loop eigenvalues for this system are stable and shown in Table 2. This simple feedback 
compensator design also has the same transfer function as the lateral component of the 


autopilot in the FROG/Autopilot plant. 


Eigenvalue _Dampin Frequency (rad/sec 
0 1.0 0 

0 1.0 0 
-0.1199 1.0 0.1199 
-0.1687 1.0 0.1687 
-0.2515 1.0 0.2515 
-0.1335+0.45141 0.2837 0.4707 
-1.0 1.0 1.0 
-2.9602 1.0 2.9602 
-3.9593 1.0 3.9593 
-1.6310+3.81231 0.3933 4.1466 
-0.8466+4.16701 0.1991 4.2521 


Table 2 CL Compensated Eigenvalues 
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2. Frequency Analysis 


With the initial control design complete, the system as shown in Figure 29 was 
linearized for robustness analysis. The Bode plot of this system is shown in Figure 30. The 
bandwidth of the system is seen to be 0.22 rads/sec. This is sufficiently below the yaw rate 
response bandwidth of 1.2 rads/sec discussed in Rivers [Ref. 3]. The stability margins with 
the feedback gain set at 75 are 3.68 dB of gain and 35° of phase margin. These are below 
the desired margins specified in the design goals, which are commonly used in controller 
design. Reducing the feedback gain to 55 achieves the specified stability margin design 
goals. 
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Figure 29 Control Loop System 
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Figure 30 Control Loop Bode Plot 


To determine the command loop bandwidth the feedback loop is closed, and the 
system is linearized as shown in Figure 31. Figure 32 shows the Bode plot of the command 


loop. The bandwidth of the system is seen to be 0.6 rads/sec, which is again below the yaw 


rate response bandwidth. 
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Figure 31 Command Loop System 
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Figure 32 Command Loop Bode Plot 






rad to deg 
2 


? 

, 

os 
16 








3. Controller Response 


No exact rise time or percent overshoot were specified for the transient response 
characteristics; however due to the confined area used for flight testing, the response must 
be sufficiently quick to capture and track the desired course in a very short time. 
Consequently, variable gains were tested to achieve the desired response. Test results will 
be detailed in Chapter VI. To summarize the results: a gain of 55 provided the desired 
stability margins; however, the transient response was such that the FROG was not be able 
to capture the line idles the turn soit was reached. A gain of 75 provided quick enough 
response, and it has adequate stability margins shown earlier. This gain was used in the 
initial flight tests. 


4. Orientation 


The feedback system consisting of the FROG, Autopilot, camera model, and vision 

controller is highly non-linear. The stability of this system depends on the chosen trim 
conditions. The position and orientation of the aircraft with respect to the line were found 
to greatly influence the stability. Obviously, a vision controller must keep the desired track 
in its field of view in order to work. In addition to this constraint, it was found that there is 
a cross track angle that will cause the system to go unstable. Cross track angles of 45°, 50°, 
55°, and 60° were tested to determine the stability region. Table 3 shows the eigenvalues 
for the 55° case. As shown, the system with this cross track angle has an unstable complex 


pair. The implementation of the controller must take this into account. It was decided to 
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impose an implementation constraint of flying the aircraft into a 45° cone of the desired 


track before engaging the vision controller. 


Eigenvalue Dampuin Frequency (rad/sec 

0 -1.0 0 

0 -1.0 0 
-0.0482 1.0 0.0482 
-0.1687 1.0 0.1687 
0.0272+0.49341 -0.0550 0.4941 
-0.6674 1.0 | 0.6674 
-1.0 1.0 1.0 
-2.9602 1.0 2.9602 
-3.9308 1.0 3.9308 
-1.6310+43.81231 0.3933 4.1466 
-0.8495+4.17401 0.1994 4.2596 


Table 3 55° Cross Track Angle 


5. Discrete Transformation 


Until now all design, testing, and analysis has been done with a continuous time 
system. For use in flight the controller must be discretized. SytemBuild: performs most of 
this automatically. Time delays were manually added to all integrators to prevent algebraic 
loops. Additionally, differentiators were approximated or transfer functions manipulated to 
eliminate the zeros. Figure 33 shows the discrete transformation of the controller after its 


dynamics have been implemented as follows: 


—O.1s+1 1] 
H(s) = sare = —0.]) 1- P (V.2) 
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After transformation the vision controller and the camera model were combined into 
a superblock for testing with the discrete model of the FROG/Autopilot plant in conjunction 
with the existing heading controller. Figure 34 shows the SystemBuild block diagram of 


the complete system used for the discrete simulations. 





Figure 33 Discrete Controller 
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Figure 34 Complete Discrete Model 


-D. CONTROLLER INTEGRATION 

A second state machine was designed to provide an automatic capture and track 
capability. This machine controls the “Capture Track” switch shown in Figure 34, which 
switches between the heading and vision controllers when the specified conditions are met. 


Figure 35 shows the state diagram block inside the “Visual Line Controller” super block. 
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Figure 35 Visual Controller Super Block 


This state machine was not incorporated into the state machine in the sensor model 
since it will be used when the actual onboard camera is operational and the sensor model is 


discarded. Figure 36 shows the diagram of the state machine. 
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Control Svitch ON 
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Figure 36 Capture/Track State Machine 
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At system initiation state one is active and the single output of the state machine is 
initialized to False. When the vision control switch is on, the machine transitions to state 
two. The output is not changed. When the range to the first line is less than 700 feet, the 
machine transitions to state three. At state three the single output is set to True by a Moore 
output. This output switches the yaw rate commanded source from the heading controller to 
the vision controller. If at any time the vision control switch is turned off, the machine 
transitions to the initialization state, and the output is reset to False causing the yaw rate 
commanded to revert to the heading controller. 

Figure 37 shows the FMS animation page used for flight testing of the vision 
controller. The operator has the same controls for heading and altitude commands, 
including the Absolute/Delta options, as detailed in Chapter Ul. The throttle controller is not 
used for these tests. Digital readouts of all the parameters are displayed. Additionally, a 
graphic display of the FROG’s position palates to the airfield and river are shown. The 
aircraft’s initial position and movement scaling are set via the three sliders titled “Xo”, 
“Yo”, and “scale”. The vision controller is engaged with the “Vision Control” switch, and 
the direction of flight is euiened by the “North/South” pushbutton. The direction of flight is 
needed for the sensor model and will be eliminated when the actual camera is used. The 
final display is a bar indicator that shows when the vision controller is operating in the 
capture state, tracking line one, line two, or line three state (Cap/L1/L2/L3). An additional 


heading display is also shown that was used for some other off-line testing. 
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VI. TEST RESULTS 


A. GROUND SIMULATION 
1. Straight Lines 


The first simulations used the single line sensor model] that was used in the 
linearization during controller development. These tests not only jalidnted the controller 
design, but they were also‘used for determining the optimum gain as discussed in Chapter 
V. Figure 38 shows the results for gains of 100, 75, and 50. All three gains are stable and 
show proper tracking of the line. Though a gain of 50 provides the desired stability 
margins, the transient response is so sluggish that the FROG would not be able to capture 
the line before the turn point is reached. A gain of 100 shows the fastest response; however, 
this would lower the stability margins below acceptable levels. 
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Figure 38 Line Tracking 
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Further gain tests were done using the simulated river model. Figure 39 shows the 
tracking error over the simulated river course for a gain of 55. Howe 40 shows the same 
for a gain of 75. The large spikes in the tracking error comespond to the state machine 
switching to a new line. A gain of 55 causes overshoots of 150 feet, and the aircraft does 
not have enough time to recapture the track during the first two legs. This performance is 
unacceptable for operational use. A gain of 75 shows overshoots of 15 feet, and the aircraft 
has plenty of time to recapture the track. A gain of 75 clearly provides a better response, 
and it has adequate stability margins listed i Chapter V. As stated previously, this gain 


was used for the initial flight testing. 
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Figure 39 = Tracking Error Gain 55 
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Figure 40 Tracking Error Gain 75 


Figure 41 shows a plot of the flight path and the control point for a simulation of the 
fully integrated vision saabaaties using the three line river model. Since the control point is 
on the desired track, it is plotted to epeceat the river. Jumps at the end of each line are a 
plotting anomaly that occurs when the control point switches to the new line. 

| The FROG was initialized 4500 feet north and 2500 feet east of the LTP origin on a 
heading of 330°. The heading and vision controllers were engaged. A heading of 210° was 
commanded via the heading controller to fly the FROG into a 45° cone of line one. At 700 
feet from line one the vision controller switched states from “Capture” to “Track” and took 
over from the heading controller. Line one was tracked until 600 feet from point two, when 


the state machine switched points to compute line two in the sensor model. A smooth turn 
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was made with 15 feet of overshoot (Figure 40) to track line two. Line two was then 
tracked until within 600 feet of point three, when the sensor model switched to line three. . 


Again a smooth turn was made with minimal overshoot to track line three. 
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Figure 41 Full River Simulation 


Figure 42 shows the sensor model and controller outputs for this simulation. The 


controller is exhibiting proper performance as evidenced by a positive commanded yaw rate 
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for a positive value of u from the pinhole camera model. Similarly, a negative yaw rate was 


commanded for a negative u value. Maximum commanded yaw rates occur at the switching 


points between the lines. Though no yaw rate over 10 deg/sec was commanded, a rate 


limiter of +10 deg/sec was added as a safety factor for actual flight tests. 
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Figure 42 Camera/Controller Output 


2. Simple Curves 


The vision controller was designed to track a single line using a linearized plant 


model ‘trimmed at a straight and level flight condition. A controller that remains stable only 


under these conditions would be of little operational value. The three line river simulation 


showed that the design is capable of tracking a more complex shape. To further check the 


a 


robustness of the controller design a commanded track for both a circle and a sine wave 


were tested. Figure 43 shows the flight path for a circle when the aircraft is initially outside 


the circle, and Figure 44 shows the flight path starting inside the circle. 


North LTP (feet) 


2000 Seana ae = ee a tees saak b chvciticee eh estaaee : 


Flight Path 
——-—- Curve 





CIO Lance gersssesseesneeebeereereetserserseesndunsesneesaenerinernetndirsersenseseepseee 
9000 ra = ea nd teciiiuencasner . 
~ $000 5000 ~ 1000 0 1000 2000 


East LTP {feet} 


Figure 43. Outside Circle 
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Figure 44 Inside Circle 


In both cases the controller was able to capture and track the desired course. Under 
these conditions the steady state tracking Bae was 83 feet on the inside of the circle. This 
error is due to the visibility distance of the control point being 1000 feet ahead of the closest 
point on the circle. As the aircraft moves forward, the control point continually moves . 
the inside of the turn. Even with u = 0 and du/dt = 0, this produces a ground track to the 


inside during any steady turn, as seen in Figure 45. 


Figure 45 Steady State Error 
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The sine wave tested the controller over a more complex course with variable radius 
turns in both directions. Figure 46 shows the flight path with the aircraft initially on the 
desired track, and Figure 47 shows the results when starting off track. Both display good 
performance and demonstrate the robustness of the controller design. 
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Figure 46 Sine Wave On Track 
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Adding noise to u at the output of the sensor model further tested the robustness of 


the controller. Simulations with Guassian noise up to 50% of the maximum noise free 


value showed that the controller can track the desired curve. Figure 48 shows the camera 
output and tracking error for a noise level of 50%. The FROG does not currently have an 


accurate IMU, so simulations with +30° of heading noise with a constant 5° bias were made 
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to ensure that the sensor model would perform during flight tests. Bank and pitch angles 
were also set to zero to test the effect of incorrect values from the IMU. Once the actual - 
camera system is installed angle errors will not be critical, since they are only used in the 


sensor model. 
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Figure 48 Camera Output/Tracking Error 


4. Wind 


Tests were done to investigate the wind effects on the controller. Figure 49 shows 
the flight path with a constant 10 knots of wind from the northeast. As can be seen, an 
offset downwind of the desired track was flown. Tests with the wind out of the north were 


also made and, as expected, showed an offset only on the crosswind leg. 
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Figure 49 10 Knot NE Wind 


This is a result of controlling the aircraft in {IM}. When the aircraft has established 
a constant ground track with zero lateral displacement in {IM}, a steady state tracking error 


is experienced as seen in Figure 50. This error is a function of the wind crab angle and the 


visibility distanced used for the control point. 
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Figure 50 Wind Induced Error 
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B. FLIGHT TESTS 
1. Flight One 


Two flight tests were made to test the controller design and to validate the use of the 
computer based camera model in the place of an actual camera. During flight tests the 
camera model uses position from the GPS and Euler angles from the IMU exactly as those 

--yalues from the FROG model superblock were used during sesund simulations. The flights 


were conducted at the Chualar RC airfield shown in Figure 19. 
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Figure 51 Initial Capture Flight Path 
Figure 51 shows the initial capture and track results of the first run. After takeoff 


the FROG was manually flown by the pilot to a point north of the field and established on a 


westerly heading to intercept the simulated river before control is passed to the computer. 
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At five seconds the trainer switch was engaged by the pilot handing over control, as 
evidenced by the yaw rate commanded shown in Figure 52. The heading controller then 
commanded a 230 heading towards the river. From the GPS tracking data shown in Figure 
51 it can be seen that the FROG was actually on a northwesterly heading. This is due to the 
poor performance of the onboard IMU. At ten seconds the range to the line had decreased 
to 700 feet, which changed states in the capture/track state machine. Bei was then 
switched to the vision controller. At the switching point the output of the sensor model 
shows the Image plane coordinate u of the control point to the left of the: aircraft. A 
negative yaw rate commanded was sent, as expected. As the FROG entered a left turn, u 
- become more negative. In other words, it was going farther to the left. This is a good 
display of the adverse yaw characteristic of the FROG. As the control point passed in front 
of the FROG the yaw rate commanded went from negative to positive exhibiting the proper 
operation. 

The initial capture attempt shown in Figures 51 and 52 demonstrates that the sensor 
model is working, that the capture/track state machine has the proper logic, and that the | 
controller is sending reasonable commands. However, the FROG was over controlled and 
unable to track the line. The run was terminated before divergence for safety reasons. This 
result can be attributed to two factors. The first reason is the smaller stability margins with 
the controller gain set at 75. The second reason is the large cross track angle when the 
vision controller was engaged. This last problem was due to both the difficulty in manually 
positioning the FROG with relation to the line from the ground and to the poor heading 


from the IMU. Further tests runs were made with similar results. - 


65 





me (seconds } 


I 





Cjassbap) pus J 


Figure 52 Initial Capture Data 
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2. ‘Flight Two 


A second flight was conducted after insertion of a sliding gain in the SystemBuild . 
model of the controller. The initial gain was set at 50 to increase the stability margins as 
discussed in Chapter V. Figure 53 shows the track of the FROG with this setting. Control 
was passed to the computer with the aircraft closer to the airfield to aid in positioning the 

| FROG visually by the pilot. The trainer switch was engaged near seint two after the sensor 


model has switched to line two. 
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Figure 53 Gain 50 Flight Path 


The sensor output, u, seen in Figure 54 shows that the line was to the right of the 
FROG and that a positive yaw rate was properly commanded to track the line. Starting at 


20 seconds the commanded yaw rate has caused the FROG to begin turning back towards 
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the line. However, by this time the range to point three has decreased to 600 feet, and the 
line point state machine switches to line three. At 38 seconds the adverse yaw characteristic 
of the FROG again caused a large spike in u and the commanded yaw rate. The controller 
handled this spike with the rate limiter and continued to command a positive yaw rate to 
regain the new track. The test run ended before a steady state track occurred due to the 


constraints of the flight test area. Additional runs were made under similar conditions 


duplicating the results. 
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VII. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 


; The objectives of this thesis as stated in the introduction were accomplished. A 
vision controller for the FROG UAV was developed and successfully tested both in the lab 
and in actual flight. An accurate model for the sensor package was designed and used 
| throughout the process. | 

Poor IMU performance and the difficulty encountered in visually positioning the 
FROG prevented the flight testing of the system over the full three line course. It also led to. 
the reduction of the gain to a level that the controller was not able to perform in the limited 
flight test area. If the initial intercept geometry could be achieved, then the higher gain may 
be closer to the proper setting. 
The decision to work in the Image plane led e a steady state tracking i with any 
crosswind pene present. Taking the output of the sensor and ransforming it to an 
Inertial reference frame is an option for eliminating this error, if accurate onboard sensors | 
are available. However, this may not be desired. The goal of a vision guided vehicle is to 
keep the path being followed in the field of view. Slow vehicles, such as UAVs, require 
large crab angles to compensate for wind to follow a given track over the ground. When the 
UAV is exactly above the desired track, a strong crosswind could produce a crab angle large 
enough that the track would be out of the field of view of the sensor. A decision must be 


made based on the mission and the sensor onboard. Of course, a movable sensor would 


69 


eliminate this concern, but it would necessitate the measurement of variable rotation angles 
to transform from {B} to {C}. 

The secondary goal of improving the Flight Management System was also 
accomplished. These improvements will enhance the safety and operational effectiveness 


of the existing controllers during flight testing of the FROG. 


B. RECOMMENDATIONS 


‘Since only two test flights were made, it is recommended to continue flight testing 
and refining the current vision controller and the sensor model. Flights along a single line 
could be used to fully test the ara of the controller in flight using variable gains. 
Adjusting the visibility distance doi also be Lautan Ultimately a more emo 
controller may be needed. 

A more accurate heading source is needed to help with 7 initial capture and hak 
problem. Changing the state machine logic ‘that Switches from the heading to the vision 
controller to include a heading check should be tested. This would ensure that the heading 
is within the stable intercept cone before switching. 

Additional flights along the iii course in a northerly direction should also be 
made. Other courses should be designed that remain within the confines of the flight test 
area. When the software design and hardware interfaces for the actual camera and digital 


image processor are complete, they should be flight tested. 
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A controller that uses the transformation of the camera output to an Inertial 
reference frame could be developed for laboratory testing. Actual flight testing, however, 


would not be productive with the existing IMU. 
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APPENDIX CONTROL POINT CALCULATIONS 


This Xmath code is used in straight line simulations by the user defined block 
titled" calc pt LTP" as described in Chapter IV. Tests to prevent division by zero are also 
‘made. Default values are supplied for these cases. 


— inputs: u; 
outputs: y; 


float u(7), y(6), num, dem, m,Xc,Yc,Xa, Ya,direction, d; 
d = 1000; 


num = u(6) - u(4); 
dem = u(5) - u(3); 


if abs(dem) < 0.001 then 
dem = 0.001*sign(dem); 
endif; 


if dem == 0.0 then 
dem = 0.001; 
endif; 


if abs(num) < 0.01 then 
num = 0.01*sign(num); | 


endif; 

if num == 0.0 then 
num = 0.01; 

endif; 


m = num/dem; 


Ye = (u(2)*m"2 + u(1)*m - u(5) *m + 0(6))/(1 + m2); 
Xc = (Yc - u(6))/m + u(5); 


if u(7) > 0.5 then 
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direction = 1.0; 
else 

direction = -1.0; 
endif; 


Xa = Xc + d*direction*cos(atan(m)); 
Ya = m*(Xa - Xc) + Yc; 


y(1) = ((Xc - u(1))*2 + (Ye - u(2))2)0.5; 
' y(2) = Xe; 

y(3) = Ye; 

y(4) = Xa, 

y(5) = Ya; 

y(6) = 0.0; 


This is the Xmath code used to simulate a circle. It uses the same methodology as 
the line calculations. 


inputs: U; 
outputs: y; 


float u(2), y(6), num, dem,Xc,Yc,Xa, Ya, r, d, theta, dtheta: 


r = 2000; 
d = 1000; 


num = u(1); 
dem = u(2); 


if abs(dem) < 0.001 then 
dem = 0.0001*sign(dem); 


endif; 

if dem == 0.0 then 
dem = 0.0001; 

endif; 


if abs(num) < 0.001 then 
num = 0.001*sign(num); 

endif; 

if num == 0.0 then 
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num = 0.001; 
endif; 


theta = atan2(num,dem); 
dtheta = d/r; 


Xc = r*cos(theta); 
Yc =r*sin(theta); 


Xa = r*cos(theta + dtheta); 
Ya =r*sin(theta + dtheta); 


y(1) = (Ke - u(1))*2 + (Ye - u(2))*2)0.5; 
y(2) = Xe; 
y(3) = Ye; 
y(4) = Xa; 
y(5) = Ya; 
y(6) = 0.0; 


This is the Xmath code for the sine wave simulations. 


inputs: U; 
outputs: y; 


float u(2), y(6), num, dem,Xc, Yc,Xa, Ya, r, d, p; | 


- d= 500; 
r = 2000; 
p = 8000; 


Xa = u(1) + 500; 
Ya = r*sin(2*3.1415*Xa/p); 


ACS Aa: 
Yc = Ya; 


y(1) = ((Ke - u(1))*2 + (Yc - u(2))42)40.5; 
y(2) = Xc; | 

y(3) = Ye; 

y(4) = Xa; 

y(5) = Ya; 

y(6) = 0.0; 
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